/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.SpeedController;
import java.util.Vector;

/**
 * lifter control class
 * @author robert
 */
public class PhoenixLifter implements PIDSource, PIDOutput {
    public SpeedController lifterMotor;
    public AnalogChannel lifterPotentiometer;
    public boolean lifterEnabled = false;
    public PIDController lifterControl;
    PhoenixSensors sensors;
    public int lifterTarget;
    public DigitalInput lifterTopLimit;
    public DigitalInput lifterBottomLimit;
    PhoenixControls controls;
    private int target;
    public Vector debugMessages = new Vector(0);
    Const constants;

    /**
     * @brief phoenix lifter
     * phoenix lifter, takes jaguars, potentiometer ports and sensors/controls
     * @param lifterMotor lifter motor object
     * @param sensors phoenix sensors
     * @param controls phoenix controls
     * @param lifterPotentiometerPort port for lifter potentiometer
     * @param lifterTopLimitPort port for lifter top limit
     * @param lifterBottomLimitPort port for bottom lifter limit
     */
    public PhoenixLifter (SpeedController armMotor, PhoenixSensors sensors,
            PhoenixControls controls, AnalogChannel lifterPotentiometer,
            DigitalInput lifterTopLimit, DigitalInput lifterBottomLimit,
            Const constants) {

        this.constants = constants;
        this.lifterMotor = armMotor;
        this.controls = controls;
        this.lifterPotentiometer = lifterPotentiometer;
        this.sensors = sensors;
        lifterControl = new PIDController(0, 0, 0, this, this);
        this.lifterTopLimit = lifterTopLimit;
        this.lifterBottomLimit = lifterBottomLimit;
        target = constants.NOLIFTEBUTTONPRESSED();
    }

    public void addMessage(String message) {
        debugMessages.addElement(message);
    }

    /**
     * raises arm to specified height
     * @param targetHeight
     */
    public void setLifterPIDRaw(int targetHeight) {

        this.lifterTarget = targetHeight;

        // PID tuning for going up and down, respectively
        if(getHeight() > targetHeight) {
            lifterControl.setPID(1, 0, 0);
        } else {
            lifterControl.setPID(1, 0, 0);
        }
        lifterControl.setSetpoint(targetHeight);
    }

    /**
     * gets PID target for lifter
     * @return lifter target height
     */
    public double getTargetHeight() {
        return lifterControl.getSetpoint();
    }

    /**
     * sets height to preset values
     * @param height
     */
    public void setLifterPreset(LifterHeight height) {

        switch(height.value) {
            case LifterHeight.kDown_val:
                target = constants.LIFTERDOWN(); break;
            case LifterHeight.kSlot_val:
                target = constants.LIFTERSLOT(); break;
            case LifterHeight.kLowShift_val:
                target = constants.LIFTERLOWSHIFT(); break;
            case LifterHeight.kLow_val:
                target = constants.LIFTERLOW(); break;
            case LifterHeight.kMiddleShift_val:
                target = constants.LIFTERMIDDLESHIFT(); break;
            case LifterHeight.kMiddle_val:
                target = constants.LIFTERMIDDLE(); break;
            case LifterHeight.kHighShift_val:
                target = constants.LIFTERHIGHSHIFT(); break;
            case LifterHeight.kHigh_val:
                target = constants.LIFTERHIGH(); break;
            default:
                target = constants.NOLIFTEBUTTONPRESSED();
        }

        /*if (Math.abs(target - getHeight()) > 10) {
            if(target - getHeight() > 0) {
                setLifterPowerRaw(1);
            } else {
                setLifterPowerRaw(-1);
            }
        }*/
    }

    /**
     * sets lifter power, scaled
     * @param input lifter power
     */
    public void setLifterPowerRaw(double input) {
        double power = 0;
        double upScale = .7;
        double downScale = .5;

        if (getLimitBottom() && input < 0) {
            power = 0;
        } else if (getLimitTop() && input > 0) {
            power = 0;
        } else if (input > 0) {
            power = input * upScale;
        } else if (input <= 0) {
            power = input * downScale;
        }

        if(lifterMotor != null) lifterMotor.set(power);
        addMessage("commanded power: " + power);

    }

    /**
     * checks wheter bottom limits are exceeded
     * @return limits exceeded
     */
    public boolean getLimitBottom() {
        if(!lifterBottomLimit.get()) {
            addMessage("bottom hard limit triggered, blocking");
            return true;
        } else if (getLifterHeight() < constants.LIFTERBOTTOMLIMIT()) {
            addMessage("bottom soft limit triggered, blocking");
            return true;
        } else {
            return false;
        }
    }

    /**
     * checks whether top limits are exceeded
     * @return limits exceeded
     */
    public boolean getLimitTop() {
        //FIXME: uncomment
        if (!lifterTopLimit.get()){
            addMessage("top hard limit triggered, blocking");
            return true;
        } else if (getLifterHeight() > constants.LIFTERTOPLIMIT()) {
            addMessage("top soft limit triggered, blocking");
            return true;
        } else {
            return false;
        }
    }

    /**
     * gets lifter height
     * @return lifter height
     */
    public int getLifterHeight() {
        return lifterPotentiometer.getAverageValue();
    }

    /**
     * sets lifter power directly, bypassing PID (PID must be disabled to use)
     * @param power
     */
    public void setLifterPower(double power) {

        double commandedPower = power * power;
        addMessage("commanded power: " + commandedPower);

        //If within the redzone of the target or limits, lower power to redzone power
        int currentHeight = getHeight();
        if ((Math.abs(currentHeight - target) < constants.LIFTERREDZONESPEED()) ||
              (Math.abs(currentHeight - constants.LIFTERBOTTOMLIMIT()) <
              constants.LIFTERREDZONESPEED()) || (Math.abs(currentHeight -
              constants.LIFTERTOPLIMIT()) < constants.LIFTERREDZONESPEED())) {
            commandedPower = .2;
        }

       // If input power is negative invert commandedPower.
        if (power < 0) commandedPower *= -1;
        //TODO: direct PID controller to use this method

       // Make sure that we don't over turn the pot on the lifter.
        // and check the upper and lower limit.
        if (getLimitBottom() && power < 0) {
            commandedPower=0;
            addMessage("bottom lifter limit exceeded, blocking");
        } else if (getLimitTop() && power > 0) {
            commandedPower=0;
            addMessage("top lifter limit exceeded, blocking");
        } else {
            if(commandedPower < 0) {
                commandedPower *= 0.5;
            }
        }

        //We know the power to set, check to see if we need go to a preset heiaght.
        if (target != constants.NOLIFTEBUTTONPRESSED()) {
            //If we are within 1 inch from the targer, we stop.
            if (Math.abs(target - getHeight()) < constants.LIFTERSTOPZONE()) commandedPower =0;
        }
        // Set the lifter motor
        if(lifterMotor != null) lifterMotor.set(commandedPower);
//        addMessage(getHeight());
//        addMessage(target);
//        addMessage(commandedPower);

    }

    /**
     * calls getHeight for PID
     * @return PID value
     */
    public double pidGet() {
        return getHeight();
    }

    /**
     * sets power from PID controller
     * @param power PID power
     */
    public void pidWrite(double power) {
        setLifterPowerRaw(power);
    }

    /**
     * gets power to lifter motor
     * @return lifte rpower
     */
    public double getPower() {
         return (lifterMotor != null ? lifterMotor.get() : 0);
    }

    /**
     * starts PID controller for lifter
     */
    public void startPID() {
        lifterControl.enable();
    }

    /**
     * stops PID controller for lifter
     */
    public void stopPID() {
        lifterControl.disable();
    }

    public boolean pidEnabled() {
        return lifterControl.isEnable();
    }

    /**
     * add/subtract value to lifter height (for use by teleop)
     * @param modifier
     */
    public void modifyHeight(double modifier) {
        double scale = 2;
        lifterTarget += modifier * scale;
    }

    /**
     * preset lifter heights
     */
    public static class LifterHeight {
        public final int value;
        private LifterHeight(int value) {
            this.value = value;
        }
        static final int kDown_val = 0;
        static final int kSlot_val = 1;
        static final int kLowShift_val = 2;
        static final int kLow_val = 3;
        static final int kMiddleShift_val = 4;
        static final int kMiddle_val = 5;
        static final int kHighShift_val = 6;
        static final int kHigh_val = 7;
        static final int kNoHeight_val = 2342;

        /**
         * lifter at docked position
         */
        public static final LifterHeight kDown =
                new LifterHeight(kDown_val);

        /**
         * lifter at loading slot
         */
        public static final LifterHeight kSlot =
                new LifterHeight(kSlot_val);

        /**
         * lifter at shifted low rung
         */
        public static final LifterHeight kLowShift =
                new LifterHeight(kLowShift_val);

        /**
         * lifter at low rung
         */
        public static final LifterHeight kLow =
                new LifterHeight(kLow_val);

        /**
         * lifter at shifted middle rung
         */

        public static final LifterHeight kMiddleShift =
                new LifterHeight(kMiddleShift_val);

        /**
         * lifter at middle rung
         */
        public static final LifterHeight kMiddle =
                new LifterHeight(kMiddle_val);

        /**
         * lifter at shifted high rung
         */
        public static final LifterHeight kHighShift =
                new LifterHeight(kHighShift_val);

        /**
         * lifter at high rung
         */
        public static final LifterHeight kHigh =
                new LifterHeight(kHigh_val);

        /**
         * lifter at high rung
         */
        public static final LifterHeight kNoHeight =
                new LifterHeight(kNoHeight_val);
    }

    /**
     * gets lifter height based on potentiometer
     * @return lifter height
     */
    public int getHeight () {
        return lifterPotentiometer.getAverageValue();
    }
}
